/*
## @file
#
#  Copyright (c) 2018 Loongson Technology Corporation Limited (www.loongson.cn).
#  All intellectual property rights(Copyright, Patent and Trademark) reserved.
#
#  Any violations of copyright or other intellectual property rights of the Loongson Technology
#  Corporation Limited will be held accountable in accordance with the law,
#  if you (or any of your subsidiaries, corporate affiliates or agents) initiate
#  directly or indirectly any Intellectual Property Assertion or Intellectual Property Litigation:
#  (i) against Loongson Technology Corporation Limited or any of its subsidiaries or corporate affiliates,
#  (ii) against any party if such Intellectual Property Assertion or Intellectual Property Litigation arises
#  in whole or in part from any software, technology, product or service of Loongson Technology Corporation
#  Limited or any of its subsidiaries or corporate affiliates, or (iii) against any party relating to the Software.
#
#  THIS SOFTWARE IS PROVIDED BY THE AUTHOR "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
#  THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR
#  BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
#  PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION).
#
#
##
*/

#include <PiPei.h>
#include <PiDxe.h>
#include <Base.h>
#include <Library/BaseLib.h>
#include <Library/DebugLib.h>
#include <Library/BaseMemoryLib.h>
#include <Library/MemoryAllocationLib.h>
#include <Protocol/PciRootBridgeIo.h>
#include <Uefi.h>
#include <Protocol/ComponentName.h>
#include <Protocol/DriverBinding.h>
#include <Protocol/PciIo.h>
#include <Protocol/PciRootBridgeIo.h>
#include <Library/UefiDriverEntryPoint.h>
#include <Library/UefiLib.h>
#include <Library/UefiBootServicesTableLib.h>
#include <IndustryStandard/Pci.h>
#include <Library/IoLib.h>
#include <Library/Ls7aI2c.h>
#include <Library/MemLib.h>
#include <Library/SpdLib.h>
#include <Library/TimerLib.h>
#include <Library/Ls7aInitLib.h>
#include <Library/PeiServicesLib.h>

#if EC_INIT_ENABLE

void ecram_display(void);

static unsigned char linux_inb(unsigned long port)
{
      return (*(volatile unsigned char *)(0x90000efdfc000000 + port));
}
#define linux_outb(port,val) \
do {\
      *(volatile unsigned char *)(0x90000efdfc000000 + (port)) = (val);  \
      } while(0)


extern UINT8  McBitMap;
VOID
EnterConfigMode (
VOID
  )
/*++

Routine Description:
  Enter Super IO Config Mode.

Arguments:

Returns:
  None

--*/  
{
  UINT8  PnpKey;

  PnpKey = 0x87;
  linux_outb(SIO_CONFIG_PORT, PnpKey);
  PnpKey = 0x01;
  linux_outb(SIO_CONFIG_PORT, PnpKey);
  PnpKey = 0x55;
  linux_outb(SIO_CONFIG_PORT, PnpKey);
  PnpKey = 0x55;
  linux_outb(SIO_CONFIG_PORT, PnpKey);
  
  return;
}

VOID
ExitConfigMode (
  VOID
  )
/*++

Routine Description:
  Exit Super IO Config Mode.

Arguments:

Returns:
  None

--*/    
{
  UINT8  RegData = 0x00;

  linux_outb (LPC_EC_INDEX_PORT, SIO_CONFIG_CTRL);
  RegData = linux_inb (LPC_EC_DATA_PORT) | BIT1;
  linux_outb (LPC_EC_INDEX_PORT, SIO_CONFIG_CTRL);  
  linux_outb (LPC_EC_DATA_PORT, RegData);
  
  return;
}

EC_init  ITE8733InitTable[] = {
/*  Value    Register */
	{ 0x07, 0x06},      /* Mouse */
	{ 0x30, 0x01},      /* Active */
	{ 0x70, 0x0C},      /* Interrupt */
        { 0x07, 0x03},
        { 0x30, 0x01},
        { 0x60, 0x03},
        { 0x61, 0x78},
        { 0x70, 0x07},
        { 0x74, 0x03},
        { 0xF0, 0x0B},
};

void IT8733_INIT(void)
{
  unsigned long Counter =0;
  unsigned char Chipid0,Chipid1 =0;
  EnterConfigMode();
  linux_outb (LPC_EC_INDEX_PORT, 0x20);  
  Chipid0 = linux_inb (LPC_EC_DATA_PORT);
   linux_outb (LPC_EC_INDEX_PORT, 0x21);  
  Chipid1 = linux_inb (LPC_EC_DATA_PORT);
  for (Counter = 0;Counter < ((sizeof(ITE8733InitTable))/(sizeof(EC_init)));Counter++)
  {
    linux_outb(LPC_EC_INDEX_PORT,(unsigned char) ITE8733InitTable[Counter].index );
    linux_outb(LPC_EC_DATA_PORT,(unsigned char) ITE8733InitTable[Counter].data ); 
   
  }
  ExitConfigMode();  
  DEBUG((EFI_D_ERROR, "Lyang: In Ls7a.c, line %d. Chipid0 %x Chipid1 %x\n", __LINE__,Chipid0,Chipid1));
  /*
  linux_outb (LPC_EC_INDEX_PORT, 0x20);  
  Chipid0 = linux_inb (LPC_EC_DATA_PORT);
   linux_outb (LPC_EC_INDEX_PORT, 0x21);  
  Chipid1 = linux_inb (LPC_EC_DATA_PORT);
//setiobase 0x62(MSB) 0x63(LSB)
  linux_outb (LPC_EC_INDEX_PORT, 0x62); 
  linux_outb (LPC_EC_DATA_PORT, 0x30);  
  linux_outb (LPC_EC_INDEX_PORT, 0x63); 
  linux_outb (LPC_EC_DATA_PORT, 0x10);  
//function selection
  linux_outb (LPC_EC_INDEX_PORT, 0x7);  
  linux_outb (LPC_EC_DATA_PORT, 0x7);  
//simple I/O function GP34(RESET) GP35
  linux_outb (LPC_EC_INDEX_PORT, 0xC2);  
  linux_outb (LPC_EC_DATA_PORT, linux_inb (LPC_EC_DATA_PORT)|0x30);  
//Output and Input settings
  linux_outb (LPC_EC_INDEX_PORT, 0xCA);  
  linux_outb (LPC_EC_DATA_PORT, linux_inb (LPC_EC_DATA_PORT)|0x30);  

  //OutPut settings
  linux_outb (0x3012, (linux_inb(0x3012) | 0x30));
*/
}

#endif//EC_INIT_ENABLE



#if defined(SIO_NCT6106_EN)&&(SIO_NCT6106_EN)
NT6102_init NT6102inittable[]= {
//	{0x007, 0x005}, //PS2 keyboard and mouse
//	{0x060, 0x000},
//	{0x061, 0x060},
//	{0x062, 0x000},
//	{0x063, 0x064},
//	{0x070, 0x001},
//	{0x072, 0x00c},
//	{0x030, 0x001},

	{0x07, 0x02},//UARTA
	{0x60, 0x03},
	{0x61, 0xe8},
	{0x70, 0x04},
	{0x30, 0x01},


	{0x07, 0x03},//UARTB
	{0x60, 0x02},
	{0x61, 0xe8},
	{0x70, 0x03},
	{0x30, 0x01},


//	{0x07, 0x10},//UARTC
//	{0x60, 0x03},
//	{0x61, 0x38},
//	{0x70, 0x04},
//	{0x30, 0x01},
//
//
//	{0x07, 0x11},//UARTD
//	{0x60, 0x02},
//	{0x61, 0x80},
//	{0x70, 0x04},
//	{0x30, 0x01},
//
//	{0x07, 0x12},//UARTE
//	{0x60, 0x03},
//	{0x61, 0xe8},
//	{0x70, 0x04},
//	{0x30, 0x01},


//	{0x07, 0x13},//UARTF
//	{0x60, 0x02},
//	{0x61, 0xe8},
//	{0x70, 0x04},
//	{0x30, 0x01},
//
//	{0x07, 0x0b},//caseopen
//	{0x60, 0x05},
//	{0x61, 0x00},
//	{0x30, 0x01},

//  {0x07, 0x07},//IO enable
//	{0xed, 0xff},
//	{0xec, 0x00},
//	{0xf0, 0x00},
//	{0xf1, 0xff},
//	{0x30, 0xdf},
};

void NT6102_INIT()
{
	unsigned char Counter=0;
	unsigned char byte = 0;
	linux_outb(SIO_ADDR, 0x87);
	linux_outb(SIO_ADDR, 0x87);

	for(Counter=0;Counter <((sizeof(NT6102inittable))/(sizeof(NT6102_init)));Counter++)
	{
		linux_outb(SIO_ADDR, (unsigned char)NT6102inittable[Counter].addr);

		linux_outb(SIO_ADDR+1, (unsigned char)NT6102inittable[Counter].data);
		DbgPrint(EFI_D_INFO, "test SuperIO data: %x\n",linux_inb(SIO_ADDR+1));
	}

	//Enter Set Golbal Reg
	linux_outb(SIO_ADDR, 0x26);
	byte = linux_inb(SIO_ADDR+1);
	byte |=0x10;//Set CR26 bit4=1
	linux_outb(SIO_ADDR, 0x26);
	linux_outb(SIO_ADDR+1, byte);

//	linux_outb(SIO_ADDR, 0x1c);
//	byte = linux_inb(SIO_ADDR+1);
//	byte |=0x0e;//Multi Function Selection UART C&D&E
//	linux_outb(SIO_ADDR, 0x1c);
//	linux_outb(SIO_ADDR+1, byte);

//		linux_outb(SIO_ADDR, 0x10);
//		byte = linux_inb(SIO_ADDR+1);
//		byte &=(~0x30);//Device UART A&B IRQ Edge TYPE Selection
//		linux_outb(SIO_ADDR, 0x10);
//		linux_outb(SIO_ADDR+1, byte);

			linux_outb(SIO_ADDR, 0x14);
			byte = linux_inb(SIO_ADDR+1);
			byte &=(~0x18);//irq3 and irq4 Polarity High
			linux_outb(SIO_ADDR, 0x14);
			linux_outb(SIO_ADDR+1, byte);

				//Exit Set Golbal Reg
			linux_outb(SIO_ADDR, 0x26);
			byte = linux_inb(SIO_ADDR+1);
			byte &=(~0x10);//Set CR26 bit4=0
			linux_outb(SIO_ADDR, 0x26);
			linux_outb(SIO_ADDR+1, byte);

			//exit SIO
			linux_outb(SIO_ADDR, 0xaa);
			DbgPrint(EFI_D_INFO, "--------> czc sio init\n");
}
#endif//SIO_NCT6106_EN
//score calculation algorithm:
//for value: 3/c: -1; 1/4/5: +1; others: +0;
//repeat N times, each time get 2 values, so highest score is 2N,
//lowest score is -2N.
//add score offset 2N to make score keep positive
//record number of value 3 and value c seperately too, for dll adjustment.
//Clk1:t1: count of value 3; indicate clk 1 eaREADLy
//Clk0:t2: count of value c; indicate clk 0 eaREADLy
//CurrentScore:t3: current configure score
//Control:t5
//Compare:a2
//ReadConfreg:a0
UINTN
Abs(
  UINTN a1,
  UINTN a2
  )
{
  return a1 > a2 ? (a1 - a2) : (a2 - a1);
}

VOID
Ls7aGetPcieDllScore(
  UINTN ConfregAddr,
  CurrentScore *Result
  )
{
  UINTN Control,Compare,ReadConfreg,Loop;
  Result->Clk1 = 0;
  Result->Clk0 = 0;
  Result->CurrentScore = PCIE_PD_LOOP * 2;
  Loop = PCIE_PD_LOOP;
  do{
    ReadConfreg = Readl(ConfregAddr);
    Compare = ((ReadConfreg >> 24 ) & 0xf);
    Control = 0;
    do{
      switch(Compare){
        case 0x3:
          Result->Clk1 += (1 << 0);
          Result->CurrentScore -= 1;
          break;
        case 0xc:
          Result->Clk0 += (1 << 0);
          Result->CurrentScore -= 1;
          break;
        case 0x1:
        case 0x4:
        case 0x5:
          Result->CurrentScore += 1;
          break;
        default:
          break;
      }
      if(Control != 0){
        break;
      }
      Control++;
      Compare = ((ReadConfreg >> 28 ) & 0xf);
    }while(1);
    Loop--;
  }while(Loop);
}

EFI_STATUS
Ht3aSideConfigure (
  IN UINT64 Base
  )
{
  UINT32 Memsize, Val32;
#if 0	//enable HT sample for debug
  Writel(0x54, 0x1);
#endif

  /* open rx space in cpu
   * window0 00_0000_0000 - 1f_ffff_ffff
   * cache DMA and uncache DMA are mutual exclusion or co-work together???
   * */
  DBGPRINT((EFI_D_ERROR,"open 3A ht rx space\n"));
  //translate 0x80000000 ~ 0x90000000 to 0x0 ~ 0x10000000 for legacy kernel
  Writel(Base + LS3A_HT_RX_CACHE_WIN0_OFFSET + 4, 0x0080fff0);
  Writel(Base + LS3A_HT_RX_CACHE_WIN0_OFFSET, 0xc0000000);

  // Memsize : 0x1000 = 4G  0x2000 = 8G  0x4000 = 16G ...
  // Resave Node 0 High Mem 512M to Gpu
  Memsize =(PcdGet64(PcdNode0MemorySize)>>20);
  DbgPrint(DEBUG_INFO,"Memsize=0x%llx\n",Memsize);
  //TODO There is no nessery to open Node1 GPU GMEM window.
  //Get GPU GMEM window Base Addr if High mem base addr is 0x90000000 below Val32 should is 0x6
  Val32 = (UINT32)(((((HIGH_MEM_BASE_ADDRESS&0xfffffffffULL)>>28)&0xff) - 0x3)<<4);
  Val32 |= 0xc0000000;
  Val32 += (Memsize >> 0x4);
  //translate 0x20000000 ~ 0x40000000 to (0xe0000000 + (Memsize << 20)) ~ (0x100000000 + (Memsize << 20))
  Writel(Base + LS3A_HT_RX_CACHE_WIN1_OFFSET + 4, 0x0020ffe0);
  Writel(Base + LS3A_HT_RX_CACHE_WIN1_OFFSET, Val32);

  //open rx window: 0x0 ~ 0x1f_ffff_ffff
  Writel(Base + LS3A_HT_RX_CACHE_WIN2_OFFSET + 4, 0x0000e000);
  Writel(Base + LS3A_HT_RX_CACHE_WIN2_OFFSET, 0x80000000);
#ifdef MULTI_CHIP
  //related to system chip number, not the 3A - 7A connetion
  //recover node 1 access address (bit 37: node id)
#ifdef LS3A4000
  Writel(Base + LS3A_HT_RX_CACHE_WIN3_OFFSET + 4, 0x00008000);
  Writel(Base + LS3A_HT_RX_CACHE_WIN3_OFFSET, 0xe0000000);
#else
  Writel(Base + LS3A_HT_RX_CACHE_WIN3_OFFSET + 4, 0x2000e000);
  Writel(Base + LS3A_HT_RX_CACHE_WIN3_OFFSET, 0xc0100000);
#endif
#endif

#if 0	//uncache DMA   currently, configured by kernel
  //translate 0x80000000 ~ 0x90000000 to 0x0 ~ 0x10000000 for legacy kernel
  Writel(Base + LS3A_HT_RX_UNCACHE_WIN0_OFFSET + 4, 0x0080fff0);
  Writel(Base + LS3A_HT_RX_UNCACHE_WIN0_OFFSET, 0xc0000000);

  Writel(Base + LS3A_HT_RX_UNCACHE_WIN1_OFFSET + 4, 0x0000e000);
  Writel(Base + LS3A_HT_RX_UNCACHE_WIN1_OFFSET, 0x80000000);

#ifdef MULTI_CHIP
  //recover node 1 access address (bit 37: node id)
  Writel(Base + LS3A_HT_RX_UNCACHE_WIN2_OFFSET + 4, 0x2000e000);
  Writel(Base + LS3A_HT_RX_UNCACHE_WIN2_OFFSET, 0xc0100000);
#endif
#endif

  //set csr_dw_write to 1'b0 to transfer write mask infomation when write data less than 32Byte
  DBGPRINT((EFI_D_INFO,"Disable 3A HT dw_write.\n"));
#ifdef LS3A4000
  Readl(Base + 0x1c0) &= 0xfbffffff;
#else
  Readl(Base + 0x50) &= 0xff7fffff;
#endif

  DBGPRINT((EFI_D_INFO,"Set 3A HT int, using 8 bit int.\n"));
#ifdef LS3A4000
  Readl(Base + 0x1c4) |= 0x00000400;
#else
  Readl(Base + 0x58) |= 0x00000400;
#endif

#ifdef LS3A4000
#ifdef ALLOW_EXT_IOI
  DBGPRINT((EFI_D_INFO,"Set 3A4000 HT EXT int allow, allow kernel to use EXT_IOI.\n"));
  Readl(Base + 0x274) |= 0x40000000;
#else
  DBGPRINT((EFI_D_INFO,"NOT set 3A4000 HT EXT int allow, forbid kernel to use EXT_IOI.\n"));
  Readl(UINT64(0x900000001fe00000) + 0x8) &= 0xfffffff7;
#endif
#endif
  return EFI_SUCCESS;
}

EFI_STATUS
Ht7aSideConfigure (
  IN UINT64 Base
  )
{
  UINT32 Tmp;

  DBGPRINT((EFI_D_ERROR,"Open 7A HT RX space\n"));
  //window0 00_0000_0000-7f_ffff_ffff
  Writel(Base + LS7A_HT_RX_WIN0_OFFSET + 4, 0);
  Writel(Base + LS7A_HT_RX_WIN0_OFFSET + 4, 0x00008000);
  Writel(Base + LS7A_HT_RX_WIN0_OFFSET, 0);
  Writel(Base + LS7A_HT_RX_WIN0_OFFSET, 0x80000000);
  DBGPRINT((EFI_D_ERROR,"Open 7A HT RX win0 space\n"));

  //window1 fd_fc00_0000-fd_fcff_ffff
  Writel(Base + LS7A_HT_RX_WIN1_OFFSET + 4, 0xfdfcffff);
  Writel(Base + LS7A_HT_RX_WIN1_OFFSET, 0x80000000);
  DBGPRINT((EFI_D_ERROR,"Open 7A HT RX win1 space\n"));

#if 0	//enable DMA post write
  DBGPRINT((EFI_D_ERROR,"Enable 7A HT Post space\n"));
  //window0 00_0000_0000-7f_ffff_ffff
  Writel(LS7A_HT_TX_POST_WIN0_OFFSET + 4, 0x00008000);
  Writel(LS7A_HT_TX_POST_WIN0_OFFSET, 0x80000000);
#endif
  //set csr_dw_write to 1'b0 to transfer write mask infomation when write data less than 32Byte
  DBGPRINT((EFI_D_ERROR,"Disable 7A HT dw_write.\n"));
  Tmp = Readl(Base + 0x1c0);
  Tmp &= 0xfbffffff;
  Writel(Base + 0x1c0, Tmp);

  //enable HT configure access
  DBGPRINT((EFI_D_ERROR,"Enable 7A HT configure access\n"));
  Writel(Base + 0x168, 0xc000fe00);

  return EFI_SUCCESS;
}

EFI_STATUS
Ls3a7aHtConfigure (
  UINT64 NodeNum
  )
{
  UINT64 i;
  UINT64 Ht3aSideBase = HT1_CONTROLLER_CONF_BASE_ADDR;
  UINT64 Ht7aSideBase = HT_CONF_TYPE0_ADDR;

#ifdef MULTI_CHIP
#ifndef LS7A_2WAY_CONNECT
  NodeNum = NodeNum - 1;
#endif
#endif
  for (i = 0; i < NodeNum; i++) {
    Ht3aSideBase |= (i << NODE_OFFSET);
    Ht7aSideBase |= (i << NODE_OFFSET);
#ifdef PRINT_HT1_REG
    UINT64 RegEnd, Reg;
    DBGPRINT((EFI_D_ERROR, "LS3A Node %d HT registers before init\n", i));
    for (Reg = Ht3aSideBase, RegEnd = Reg + 0x180; Reg < RegEnd; Reg += 0x4) {
      DBGPRINT((EFI_D_ERROR, "0x%llx: 0x%016lx\n", Reg, Readl(Reg)));
    }

    DBGPRINT((EFI_D_ERROR, "LS7A to Node %d HT registers before init\n", i));
    for (Reg = Ht7aSideBase, RegEnd = Reg + 0x268; Reg < RegEnd; Reg += 0x4) {
      DBGPRINT((EFI_D_ERROR, "0x%llx: 0x%016lx\n", Reg, Readl(Reg)));
    }
#endif

    //3A side HT init
    Ht3aSideConfigure(Ht3aSideBase);

    //7A side HT configure begin
    Ht7aSideConfigure(Ht7aSideBase);
#ifdef PRINT_HT1_REG
    DBGPRINT((EFI_D_ERROR, "LS3A Node %d HT Registers after init\n", i));
    for (Reg = Ht3aSideBase, RegEnd = Reg + 0x180; Reg < RegEnd; Reg += 0x4) {
      DBGPRINT((EFI_D_ERROR, "0x%llx: 0x%016lx\n", Reg, Readl(Reg)));
    }

    DBGPRINT((EFI_D_ERROR, "LS7A to Node %d HT Registers after init\n", i));
    for (Reg = Ht7aSideBase, RegEnd = Reg + 0x268; Reg < RegEnd; Reg += 0x4) {
      DBGPRINT((EFI_D_ERROR, "0x%llx: 0x%016lx\n", Reg, Readl(Reg)));
    }
#endif
  }
  return EFI_SUCCESS;
}

EFI_STATUS
ConfigOnePll (
  IN UINT64 PllBase,
  IN UINT32 PllVal,
  IN UINT32 DivRefc
  )
{
  UINT32 i,Val32;
  Readl(PllBase + 0x4) &= ~(0x7 << LS7A_PLL_SEL0_OFFSET);//switch to backup clk
  Readl(PllBase + 0x4) |= (1 << LS7A_PLL_PD_OFFSET);//power down pll
  Readl(PllBase + 0x4) &= ~(1 << LS7A_PLL_SET_OFFSET);//disable pll configure

//configure pll parameters
  Readl(PllBase) = PllVal;
  Val32 = Readl(PllBase + 0x4);
  Val32 = (Val32 & ~(0x3f << LS7A_PLL_DIV_REFC_OFFSET)) | DivRefc;
  Readl(PllBase + 0x4) = Val32;

  Readl(PllBase + 0x4) |= (1 << LS7A_PLL_SET_OFFSET);//enable pll configure
  Readl(PllBase + 0x4) &= ~(0x1 << LS7A_PLL_BYPASS_OFFSET);//not bypass pll
  Readl(PllBase + 0x4) &= ~(0x1 << LS7A_PLL_PD_OFFSET);//power up pll


//poll lock signal
  for (i = 0; i < 0x1000; i++) {
    if (Readl(PllBase + 0x4) & (0x1 << LS7A_PLL_LOCK_OFFSET)) {
      //select pll out
      Readl(PllBase + 0x4) |= (0x7 << LS7A_PLL_SEL0_OFFSET);
      break;
    }
  }
  if (i == 0x1000)
    return EFI_INVALID_PARAMETER;

  return EFI_SUCCESS;
}

EFI_STATUS
Ls7aPhyCfgWrite (
  IN UINT64    CfgAddr,
  IN UINT32    Data
  )
{
  UINT32 i = 0;
  while (i < 0x100000) {
    if (Readl(CfgAddr + 0x4) & 0x4)
      break;
    MicroSecondDelay(100);
    i++;
  }
  if (i == 0x100000) {
    DbgPrint(EFI_D_INFO, "Ls7aPhyCfgWrite TimeOut\r\n");
    return EFI_INVALID_PARAMETER;
  }

  Readl(CfgAddr) = Data;
  Readl(CfgAddr + 0x4) = 0x1;
  return EFI_SUCCESS;
}

UINT32
Ls7aPhyCfgRead (
  IN UINT64   CfgAddr,
  IN UINT32   Data
  )
{
  UINT32 i = 0;
  while (i < 0x100000) {
    if (Readl(CfgAddr + 0x4) & 0x4)
      break;
    i++;
  }
  if (i == 0x100000) {
    DbgPrint(EFI_D_INFO, "Ls7aPhyCfgRead TimeOut\r\n");
    return -1;
  }

  Readl(CfgAddr) = Data;
  Readl(CfgAddr + 0x4) = 0x0;

  while (i < 0x100000) {
    if (Readl(CfgAddr + 0x4) & 0x4)
      break;
    i++;
  }
  if (i == 0x100000) {
    DbgPrint(EFI_D_INFO, "Ls7aPhyCfgRead TimeOut\r\n");
    return -1;
  }

  return Readl(CfgAddr);
}

EFI_STATUS
DeviceCfg (
  OUT Ls7aPcieInfo *PcieInfo
  )
{
  UINT32 Val32, Data;
  UINT64 i = 0;
  INT32 WaitTime = 0;
  UINT8 GMemModuleType;
  UINTN OldScore;
  CurrentScore Result;
  UINTN ConfregAddr,RightShift,AdjustValue;
  ConfregAddr = 0x5cc + LS7A_CONFBUS_BASE_ADDR;

//init PCIE -- v0.3
//configure phy parameter
  for (i = 0; i < 0xa0; i += 0x20) {
    Readl(LS7A_CONFBUS_BASE_ADDR + 0x580 + i) = 0xc2492331;
  }
  for (i = 0; i < 0xa0; i += 0x20) {
    Readl(LS7A_CONFBUS_BASE_ADDR + 0x584 + i) = 0xff3ff0a8;
    //Readl(LS7A_CONFBUS_BASE_ADDR + 0x584 + i) = 0x73e70b0;
  }

  Readl(LS7A_CONFBUS_BASE_ADDR + 0x584) = 0x73e70b0; //pcie f0

  for (i = 0; i < 0xa0; i += 0x20) {
    Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F0_REG_OFFSET + i) = 0x27fff;
  }
//assert reset
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET) |= ((1 << 28) | (1 << 24) | (1 << 20) | (1 << 16) | (1 << 8));

#if 1//ndef USE_PCIE_PAD_REFCLK
  for (i = 0; i < 0x40; i += 0x20) {
    Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F0_REG_OFFSET + i) |= (0x1 << 27);
  }
  for (i = 0x40; i < 0xa0; i += 0x20) {
    Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F0_REG_OFFSET + i) |= (0x3 << 26);
  }
  //reset PCIE begin
  for (i = 0; i < 0xa0; i += 0x20) {
    Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F0_REG_OFFSET + i) |= (0x1 << 24);
  }
//switch clk
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET) |= (1 << 2);
  MicroSecondDelay(10000);
//power up phy
  for (i = 0; i < 0xa0; i += 0x20) {
    Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F0_REG_OFFSET + i) &= ~(0x1 << 24);
  }
//deassert reset
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET) &= ~((1 << 28) | (1 << 24) | (1 << 20) | (1 << 16) | (1 << 8));

  do{
    if(((Readl(LS7A_CONFBUS_BASE_ADDR + 0x424) >> 8) & 0xfff) == 0xfff)
     break;
  }while(1);

  MicroSecondDelay(100000);
//reset PCIE end
#ifndef FORCE_ENABLE_PCIE_F0_P123
    Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F0_REG_OFFSET) &= ~((0x1 << 27));
#endif
#ifndef FORCE_ENABLE_PCIE_F1_P1
    Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F1_REG_OFFSET) &= ~((0x1 << 27));
#endif
#ifndef FORCE_ENABLE_PCIE_H_P1
    Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_H_REG_OFFSET) &= ~((0x1 << 27));
#endif
#ifndef FORCE_ENABLE_PCIE_G0_P1
    Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_G0_REG_OFFSET) &= ~((0x1 << 27));
#endif
#ifndef FORCE_ENABLE_PCIE_G1_P1
    Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_G1_REG_OFFSET) &= ~((0x1 << 27));
#endif
#endif

//assert reset
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET) |= ((1 << 28) | (1 << 24) | (1 << 20) | (1 << 16) | (1 << 8));
//deassert reset
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET) &= ~((1 << 28) | (1 << 24) | (1 << 20) | (1 << 16) | (1 << 8));

#if 1
  //for PCIE_H/G0/G1
  //
  // RightShift:t7: adjust dir(dll delay offset, 0: add delay at dll 0; 8: add deley at dll 1)
  // OldScore:t8: old score
  //

  do{
    Ls7aGetPcieDllScore(ConfregAddr,&Result);
    //find max of value 3 and c and determine the calibration direction
    //a0:AdjustValue;
    //when the two number has remarkable difference, start dll calibration
    if(Abs(Result.Clk1,Result.Clk0) >= (PCIE_PD_LOOP / 2)){
      RightShift = 0;
      if(Result.Clk1 > Result.Clk0)
        RightShift = 8;

      do{
        OldScore = Result.CurrentScore;
        AdjustValue = ((Readl(ConfregAddr) >> RightShift) & 0xff);
        if( AdjustValue == 0xff)
          break;
        AdjustValue =(((AdjustValue << 1 ) | 0x1 ) & 0xff) << RightShift;
        Readl(ConfregAddr) = AdjustValue;
        Ls7aGetPcieDllScore(ConfregAddr,&Result);

        //if this calibration has not make it remarkable worse, continue
      }while((Result.CurrentScore - OldScore) > -2);
      //make it worse a lot, scroll back
      Readl(ConfregAddr) = ((((Readl(ConfregAddr) >> RightShift ) & 0xff ) >> 1 ) << RightShift);
    }
    ConfregAddr += 0x20;
  }while(ConfregAddr <= (0x60c + LS7A_CONFBUS_BASE_ADDR));

#endif

//enable access
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET) |= ((1 << 29) | (1 << 25) | (1 << 21) | (1 << 17) | (1 << 9));
  DbgPrint(EFI_D_INFO, "PCIE enabled\r\n");
  MicroSecondDelay(10000);

//use t0 as global variable
//configure 7A sata PHY parameter
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA0_REG_OFFSET+0x4) = 0x30c31cf9;
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA1_REG_OFFSET+0x4) = 0x30c31cf9;
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA2_REG_OFFSET+0x4) = 0x30c31cf9;

  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA0_REG_OFFSET) = 0xf3000403;
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA1_REG_OFFSET) = 0xf3000403;
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA2_REG_OFFSET) = 0xf3000403;
#if (!LS7A_SATA0_DISABLE)
#if 1//ndef USE_SATA_PAD_REFCLK
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA0_REG_OFFSET+0x4) |= (1 << 31);
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA0_REG_OFFSET) |= (1 << 2);
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA0_REG_OFFSET) &= ~(1 << 1);
  DbgPrint(EFI_D_INFO, "%s %d\r\n",__func__,__LINE__);
  MicroSecondDelay(10000);
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA0_REG_OFFSET+0x4) &= ~(1 << 31); //power up phy
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA0_REG_OFFSET) &= ~(1 << 2); //deassert phy reset
  DbgPrint(EFI_D_INFO, "%s %d\r\n",__func__,__LINE__);
  MicroSecondDelay(10000);
#endif
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET + 0x4) &= ~(1 << 8);//deassert cntl reset
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET + 0x4) |= (1 << 10);//sata en
#if OVRD_SATA_PHY
  Ls7aPhyCfgWrite(LS7A_CONFBUS_BASE_ADDR + CONF_SATA0_PHY_OFFSET, 0x403f1002);//configure phy
#endif
  MicroSecondDelay(10000);
  DbgPrint(EFI_D_INFO, "%s %d\r\n",__func__,__LINE__);
  DbgPrint(EFI_D_INFO, "SATA0 enabled\r\n");
#else
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET + 0x4) &= ~(1 << 11);//deassert cntl reset
  DbgPrint(EFI_D_INFO, "SATA0 clock disabled\r\n");
#endif

#if (!LS7A_SATA1_DISABLE)
#ifndef USE_SATA_PAD_REFCLK
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA1_REG_OFFSET + 0x4) |= (1 << 31);
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA1_REG_OFFSET) |= (1 << 2);
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA1_REG_OFFSET) &= ~(1 << 1);
  MicroSecondDelay(10000);
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA1_REG_OFFSET + 0x4) &= ~(1 << 31); //power up phy
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA1_REG_OFFSET) &= ~(1 << 2); //deassert phy reset
  MicroSecondDelay(10000);
#endif
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET + 0x4) &= ~(1 << 12);//deassert cntl reset
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET + 0x4) |= (1 << 14);//sata en
#if OVRD_SATA_PHY
  Ls7aPhyCfgWrite(LS7A_CONFBUS_BASE_ADDR + CONF_SATA1_PHY_OFFSET, 0x403f1002);//configure phy
#endif
  MicroSecondDelay(10000);
  DbgPrint(EFI_D_INFO, "SATA1 enabled\r\n");
#else
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET + 0x4) &= ~(1 << 15);//deassert cntl reset
  DbgPrint(EFI_D_INFO, "SATA1 clock disabled\r\n");
#endif

#if (!LS7A_SATA2_DISABLE)
#ifndef USE_SATA_PAD_REFCLK
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA2_REG_OFFSET + 0x4) |= (1 << 31);
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA2_REG_OFFSET) |= (1 << 2);
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA2_REG_OFFSET) &= ~(1 << 1);
  DbgPrint(EFI_D_INFO, "%s %d\r\n",__func__,__LINE__);
  MicroSecondDelay(10000);
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA2_REG_OFFSET + 0x4) &= ~(1 << 31); //power up phy
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SATA2_REG_OFFSET) &= ~(1 << 2); //deassert phy reset
  DbgPrint(EFI_D_INFO, "%s %d\r\n",__func__,__LINE__);
  MicroSecondDelay(10000);
#endif
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET + 0x4) &= ~(1 << 16);//deassert cntl reset
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET + 0x4) |= (1 << 18);//sata en
#if OVRD_SATA_PHY
  Ls7aPhyCfgWrite(LS7A_CONFBUS_BASE_ADDR + CONF_SATA2_PHY_OFFSET, 0x403f1002);//configure phy
#endif
  MicroSecondDelay(10000);
  DbgPrint(EFI_D_INFO, "%s %d\r\n",__func__,__LINE__);
  DbgPrint(EFI_D_INFO, "SATA2 enabled\r\n");
#else
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET + 0x4) &= ~(1 << 19);//deassert cntl reset
  DbgPrint(EFI_D_INFO, "%s %d\r\n",__func__,__LINE__);
  DbgPrint(EFI_D_INFO, "SATA2 clock disabled\r\n");
#endif
//casey 
  //
  // this delay time must long than others
  //
  MicroSecondDelay(30000);
#ifdef  USE_USB_SYS_REFCLK
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET) |= (0x2 << 2);//switch refclk
  DbgPrint(EFI_D_INFO, "%s %d\r\n",__func__,__LINE__);
  MicroSecondDelay(10000);
#endif
#if (!LS7A_USB0_DISABLE)
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET) &= ~(0x1 << 9);//deassert phy reset
  DbgPrint(EFI_D_INFO, "%s %d\r\n",__func__,__LINE__);
  MicroSecondDelay(10000);
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET) &= ~(0x1 << 8);//deassert cntl reset
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET) |= (0x1 << 10);
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_USB0_PHY_OFFSET) |= (0x1 << 11);
  MicroSecondDelay(10000);
  DbgPrint(EFI_D_INFO, "%s %d\r\n",__func__,__LINE__);
  DbgPrint(EFI_D_INFO, "USB0 enabled\r\n");
#else
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET) &= ~(0x1 << 11);//disable clock
  DbgPrint(EFI_D_INFO, "%s %d\r\n",__func__,__LINE__);
  DbgPrint(EFI_D_INFO, "USB0 clock disabled\r\n");
#endif

#if (!LS7A_USB1_DISABLE)
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET) &= ~(0x1 << 13);//deassert phy reset
  DbgPrint(EFI_D_INFO, "%s %d\r\n",__func__,__LINE__);
  MicroSecondDelay(10000);
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET) &= ~(0x1 << 12);//deassert cntl reset
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET) |= (0x1 << 14);
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_USB1_PHY_OFFSET) |= (0x1 << 11);
  DbgPrint(EFI_D_INFO, "USB1 enabled\r\n");
#else
  MicroSecondDelay(10000);
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET) &= ~(0x1 << 15);//disable clock
  DbgPrint(EFI_D_INFO, "USB1 clock disabled\r\n");
#endif

// GMEM configure -- put to Device initial code ?
//casey GMAC0 and GMAC1 disable
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET) &= ~(0x1 << 4);//GMAC0 disable flow control
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET) &= ~(0x1 << 5);//GMAC0 disable clock  
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET) &= ~(0x1 << 6);//GMAC1 disable flow control
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET) &= ~(0x1 << 7);//GMAC1 disable clock

//casey
#if (!LS7A_LPC_DISABLE)
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET + 0x4) |= (0x1 << 0);
  DbgPrint(EFI_D_INFO, "LPC enabled\r\n");
#endif

#define PCIE_TRY_GEN2 1
#define PCIE_TX_FULL_SWING 0
  MicroSecondDelay(10000);
//PCIE F0
//re-configure PCIE PHYs
//DO not destroy a0, a1, for example, do not add print between these code
  Data = 0;
  for (i = 0; i < 0x400; i += 0x100) {
    Ls7aPhyCfgWrite(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F0_PHY_OFFSET, 0x4fff1002 + i);
  }
  LS7A_PCIE_CFG(0x4800, 0x60000000)
    Data |= 0x1;

  //read x4_mode to decide whether P1~P3 is valid
  if(((LS7A_PCIE_CFG_READ(0x60000000,0x28) >> 26) & 1))
    Data |= 0xe;
  LS7A_PCIE_CFG_DONE(0x4800);
  PcieInfo -> PcieCfgBuffer[(PcieInfo -> Num)++] = 0x4800;

  if(!(Data > 0x1)){

    //Port 1
    LS7A_PCIE_CFG(0x5000, 0x60100000)
      Data |= 0x2;

    LS7A_PCIE_CFG_DONE(0x5000);
    PcieInfo -> PcieCfgBuffer[(PcieInfo -> Num)++] = 0x5000;

    //Port 2
    LS7A_PCIE_CFG(0x5800, 0x60200000)
      Data |= 0x4;

    LS7A_PCIE_CFG_DONE(0x5800);
    PcieInfo -> PcieCfgBuffer[(PcieInfo -> Num)++] = 0x5800;

    //Port 3
    LS7A_PCIE_CFG(0x6000, 0x60300000)
      Data |= 0x8;

    LS7A_PCIE_CFG_DONE(0x6000);
    PcieInfo -> PcieCfgBuffer[(PcieInfo -> Num)++] = 0x6000;
  }

  if(Data ==  0xf){
    Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F0_REG_OFFSET) |= (0x1 << 24);
    Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET) &= (~(1 << 9));
    DbgPrint(EFI_D_INFO, "Powerdown PCIE F0 PHY and disable all Ports\n");
  }

  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET) &= (~(Data << 10));
  DbgPrint(EFI_D_INFO, "unused PCIE F0 ports clock disabled,%x\n",(~(Data << 10)));

//PCIE F1
  //some port is used
  //re-configure PHYs
  Data = 0;
  for (i = 0; i < 0x400; i += 0x100) {
    Ls7aPhyCfgWrite(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F1_PHY_OFFSET, 0x4fff1002 + i);
  }
  LS7A_PCIE_CFG(0x6800, 0x60000000)
    Data |= 0x1;
  //read x4_mode to decide whether P1 is valid
  if (((LS7A_PCIE_CFG_READ(0x60000000,0x28) >> 26) & 1))
    //x4 mode
    Data |= 0x2;
  LS7A_PCIE_CFG_DONE(0x6800);
  PcieInfo -> PcieCfgBuffer[(PcieInfo -> Num)++] = 0x6800;

  if (!(Data >  0x1)) {
    //Port 1
    LS7A_PCIE_CFG(0x7000, 0x60100000)
      Data |= 0x2;
    LS7A_PCIE_CFG_DONE(0x7000);
    PcieInfo -> PcieCfgBuffer[(PcieInfo -> Num)++] = 0x7000;
  }
  if (Data ==  0x3) {
    Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_F1_REG_OFFSET) |= (0x1 << 24);
    Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET) &= (~(1 << 17));
    DbgPrint(EFI_D_INFO, "Powerdown PCIE F1 PHY and disable all Ports\n");
  }
  //disable clock of unused PCIE ports
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET) &= (~(Data << 18));
  DbgPrint(EFI_D_INFO, "unused PCIE F1 ports clock disabled %x\n",(~(Data << 18)));

//PCIE H
    //some port is used
    //re-configure PHYs
  Data = 0;
  for (i = 0; i < 0x400; i += 0x100) {
    Ls7aPhyCfgWrite(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_H_PHY_LO_OFFSET, 0x4fff1002 + i);
  }
  for (i = 0; i < 0x400; i += 0x100) {
    Ls7aPhyCfgWrite(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_H_PHY_HI_OFFSET, 0x4fff1002 + i);
  }
  LS7A_PCIE_CFG(0x9800, 0x60000000)
  Data |= 0x1;
  //read x4_mode to decide whether P1 is valid
  if (!((LS7A_PCIE_CFG_READ(0x60000000,0x28) >> 26) & 1))
    //x8 mode
    Data |= 0x2;
  LS7A_PCIE_CFG_DONE(0x9800);
  PcieInfo -> PcieCfgBuffer[(PcieInfo -> Num)++] = 0x9800;

  if (!(Data > 0x1)) {
    //Port 1
    LS7A_PCIE_CFG(0xa000, 0x60100000)
      Data |= 0x2;
    LS7A_PCIE_CFG_DONE(0xa000);
    PcieInfo -> PcieCfgBuffer[(PcieInfo -> Num)++] = 0xa000;
  }
  if (Data ==  0x3) {
    Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_H_REG_OFFSET) |= (0x3 << 24);
    Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET) &= (~(1 << 21));
    DbgPrint(EFI_D_INFO, "Powerdown PCIE H PHY and disable all Ports\n");
  }
  //disable clock of unused PCIE ports
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET) &= (~(Data << 22));
  DbgPrint(EFI_D_INFO, "unused PCIE H ports clock disabled %x\n",(~(Data << 22)));

//PCIE G0
  //some port is used
  //re-configure PHYs
  Data = 0;
  for (i = 0; i < 0x400; i += 0x100) {
    Ls7aPhyCfgWrite(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_G0_PHY_LO_OFFSET, 0x4fff1002 + i);
  }
  for (i = 0; i < 0x400; i += 0x100) {
    Ls7aPhyCfgWrite(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_G0_PHY_HI_OFFSET, 0x4fff1002 + i);
  }

  LS7A_PCIE_CFG(0x7800, 0x60000000)
  Data |= 0x1;
  //read x4_mode to decide whether P1 is valid
  if (!((LS7A_PCIE_CFG_READ(0x60000000,0x28) >> 26) & 1))
    Data |= 0x2;
  LS7A_PCIE_CFG_DONE(0x7800);
  PcieInfo -> PcieCfgBuffer[(PcieInfo -> Num)++] = 0x7800;

  if (!(Data > 0x1)) {
    //Port 1
    LS7A_PCIE_CFG(0x8000, 0x60100000)
      Data |= 0x2;
    LS7A_PCIE_CFG_DONE(0x8000);
    PcieInfo -> PcieCfgBuffer[(PcieInfo -> Num)++] = 0x8000;
  }
  if (Data ==  0x3) {
    Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_G0_REG_OFFSET) |= (0x3 << 24);
    Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET) &= (~(1 << 25));
    DbgPrint(EFI_D_INFO, "Powerdown PCIE G0 PHY and disable all Ports\n");
  }
  //disable clock of unused PCIE ports
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET) &= (~(Data << 26));
  DbgPrint(EFI_D_INFO, "unused PCIE G0 port clock disabled %x\n",(~(Data << 26)));

//PCIE G1
  //some port is used
  //re-configure PHYs
  Data = 0;
  for (i = 0; i < 0x400; i += 0x100) {
    Ls7aPhyCfgWrite(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_G1_PHY_LO_OFFSET, 0x4fff1002 + i);
  }
  for (i = 0; i < 0x400; i += 0x100) {
    Ls7aPhyCfgWrite(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_G1_PHY_HI_OFFSET, 0x4fff1002 + i);
  }
  LS7A_PCIE_CFG(0x8800, 0x60000000)
    Data |= 0x1;
  //read x4_mode to decide whether P1 is valid
  if (!((LS7A_PCIE_CFG_READ(0x60000000,0x28) >> 26) & 1))
    Data |= 0x2;
  LS7A_PCIE_CFG_DONE(0x8800);
  PcieInfo -> PcieCfgBuffer[(PcieInfo -> Num)++] = 0x8800;

  if (!(Data > 0x1)) {
    //Port 1
    LS7A_PCIE_CFG(0x9000, 0x60100000)
      Data |= 0x2;
    LS7A_PCIE_CFG_DONE(0x9000);
    PcieInfo -> PcieCfgBuffer[(PcieInfo -> Num)++] = 0x9000;
  }
  if (Data ==  0x3) {
    Readl(LS7A_CONFBUS_BASE_ADDR + CONF_PCIE_G1_REG_OFFSET) |= (0x3 << 24);
    Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET) &= (~(1 << 29));
    DbgPrint(EFI_D_INFO, "Powerdown PCIE G1 PHY and disable all Ports\n");
  }
  //disable clock of unused PCIE ports
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET) &= (~(Data << 30));
  DbgPrint(EFI_D_INFO, "unused PCIE G1 ports clock disabled %x\n",(~(Data << 30)));
#ifdef LS7A_GMEM_CFG
  UINT64              *GmemParam = NULL;
  EFI_STATUS          Status;
  PEI_GET_MEMPARAM    GetMemParamPei;
  volatile UINT64     *pMcRegsBase = NULL;
	//
	//this code if should be add, is at: pmon/arch/mips/ls7a/ls7a_init.S
	//
  Readl(GPU_HEADER_ADDR + 0x18) = TEMP_GMEM_ADDR;
  Readl(GPU_HEADER_ADDR + 0x1c) = 0x0;
  Readl(GPU_HEADER_ADDR + 0x04) = 0x2;

  DbgPrint(EFI_D_INFO, "Enable gmem config space\r\n");
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET + 0x4) &= ~(0x1 << 24);

  //
  // Locate PeiMemParamPpi
  //
  Status = PeiServicesLocatePpi (
             &gEfiPeiMemParamPpiGuid,
             0,
             NULL,
             (VOID **) &GetMemParamPei
             );
  if (EFI_ERROR (Status))
    return EFI_UNSUPPORTED;

  GetMemParamPei(NULL, NULL, NULL, &GmemParam);

  for (i=0;i<MC_REGS_COUNT;i++){
    pMcRegsBase = (UINT64 *)(LS7A_GMEM_TEMP_ADDR + (i<<3));
    *pMcRegsBase = GmemParam[i];
  }

  Readb(LS7A_GMEM_TEMP_ADDR + 0x18) |= 0x1;
#if 0
  DebugPrint(EFI_D_INFO, "The GMEM parameter is:\r\n");
  for (i=0;i<MC_REGS_COUNT;i++) {
    pMcRegsBase = (UINT64 *)(LS7A_GMEM_TEMP_ADDR + (i<<3));
    UINT64 data = *pMcRegsBase;
    DebugPrint(EFI_D_INFO, "%03x: %016lx\r\n", pMcRegsBase, data);
  }
#endif

  while (!Readb(LS7A_GMEM_TEMP_ADDR + 0x163)){
    /* Just for delay */
    for (i=0; i<0x1000; i++);
  }

  GMemModuleType = 0x0;//RDIMM is 0x1; UDIMM is 0x2, here send 0x0 to leveling; otherwise, the 7A's GPU will worked unnormal.
  DDR3Leveling(LS7A_GMEM_TEMP_ADDR, 0x0, GMemModuleType, 2, 0x0);

#if 1
  DebugPrint(EFI_D_INFO, "The GMEM after parameter is:\r\n");
  for (i=0;i<MC_REGS_COUNT;i++) {
    pMcRegsBase = (UINT64 *)(LS7A_GMEM_TEMP_ADDR + (i<<3));
    UINT64 data = *pMcRegsBase;
    DebugPrint(EFI_D_INFO, "%03x: %016lx\r\n", pMcRegsBase, data);
  }
#endif

  DbgPrint(EFI_D_INFO, "Disable gmem config space\r\n");
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET + 0x4) |= (0x1 << 24);

  //set gmem space bar mask
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_GMEM_BAR_MASK_OFFSET) = PcdGet32(PcdIntegratedGraphicsPciMemSize);
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_GMEM_BAR_MASK_OFFSET + 0x4) = 0x0;


  UINT64 GmemAddr = LS7A_GMEM_TEMP_ADDR;
  Write64(GmemAddr+0x00, 0x5555555555555555);
  Write64(GmemAddr+0x08, 0xaaaaaaaaaaaaaaaa);
  Write64(GmemAddr+0x10, 0x3333333333333333);
  Write64(GmemAddr+0x18, 0xcccccccccccccccc);
  Write64(GmemAddr+0x20, 0x7777777777777777);
  Write64(GmemAddr+0x28, 0x8888888888888888);
  Write64(GmemAddr+0x30, 0x1111111111111111);
  Write64(GmemAddr+0x38, 0xeeeeeeeeeeeeeeee);

  Readl(GPU_HEADER_ADDR + 0x04) = 0;
  Readl(GPU_HEADER_ADDR + 0x18) = 0;
  Readl(GPU_HEADER_ADDR + 0x1c) = 0;

#endif

  return EFI_SUCCESS;
}

EFI_STATUS
Ls7aResourceConfigure (
  OUT Ls7aPcieInfo *PcieInfo
  )
{
  UINT64 i = 0;

#ifdef LS7A_2WAY_CONNECT
// support 2 Nodes
// route Node0 DMA to 7A HT Lo
// route Node1 DMA to 7A HT Hi
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_HT_ROUTE_OFFSET) &= ~(0xf | (0xf << 16));
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_HT_ROUTE_OFFSET) |= (0x1 | (0xa << 16));
  DbgPrint(EFI_D_INFO, "config two nodes to 7A dma route done.\r\n");
#else
// support 1 Node
// route Node0 DMA to 7A HT Lo
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_HT_ROUTE_OFFSET) &= ~(0xf | (0xf << 16));
#ifdef MULTI_CHIP
// support 2 Nodes
// route Node0 Node1 DMA to 7A HT Lo
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_HT_ROUTE_OFFSET) |= 0x1;
#endif
  DbgPrint(EFI_D_INFO, "config 7A dma route done.\r\n");
#endif

#ifndef LS7A_2WAY_CONNECT
//shut down LS7A HT Hi
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_HT_CLKEN_OFFSET) &= ~(0x1 << 1);
  DbgPrint(EFI_D_INFO, "7A HT Hi clock disabled.\r\n");
#endif

#if 1//def  LS7A_UC_ACC
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_NB_OFFSET + 0x4) |= (0x3f << 0);
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_SB_OFFSET) |= (0xef << 24);
  DbgPrint(EFI_D_INFO, "LS7A uncache accellerator enabled\r\n");
#endif

//configure 7A pll
//pcie, gmac, sata/usb
  if (ConfigOnePll(CONF_PLL0_OFFSET + LS7A_CONFBUS_BASE_ADDR, LS7A_PLL_VALUE(80, 8, 16, 12), 0x4)) {
    DbgPrint(EFI_D_INFO, "!!!LS7A PLL0 soft configure fail.\r\n");
    return EFI_INVALID_PARAMETER;
  }
//gpu, gmem, dc
  //if (ConfigOnePll(CONF_PLL1_OFFSET + LS7A_CONFBUS_BASE_ADDR, LS7A_PLL_VALUE(100, 5, 4, 10), 0x4)) {
  if (ConfigOnePll(CONF_PLL1_OFFSET + LS7A_CONFBUS_BASE_ADDR, LS7A_PLL_VALUE(127, 8, 6, 12), 0x4)) {
    DbgPrint(EFI_D_INFO, "!!!LS7A PLL1 soft configure fail.\r\n");
    return EFI_INVALID_PARAMETER;
  }
//flex, node, hda bitclk
  if (ConfigOnePll(CONF_PLL2_OFFSET + LS7A_CONFBUS_BASE_ADDR, LS7A_PLL_VALUE(96, 72, 6, 100), 0x4)) {
    DbgPrint(EFI_D_INFO, "!!!LS7A PLL2 soft configure fail.\r\n");
    return EFI_INVALID_PARAMETER;
  }
//PIX0, default 38.2MHz for x800x600
//PIX1, default 38.2MHz for x800x600
  if (ConfigOnePll(CONF_PLL3_OFFSET + LS7A_CONFBUS_BASE_ADDR, LS7A_PLL_VALUE(104, 68, 68, 68), 0x5)) {
    DbgPrint(EFI_D_INFO, "!!!LS7A PLL3 soft configure fail.\r\n");
    return EFI_INVALID_PARAMETER;
  }
  if (ConfigOnePll(CONF_PLL4_OFFSET + LS7A_CONFBUS_BASE_ADDR, LS7A_PLL_VALUE(104, 68, 68, 68), 0x5)) {
    DbgPrint(EFI_D_INFO, "!!!LS7A PLL4 soft configure fail.\r\n");
    return EFI_INVALID_PARAMETER;
  }
  DbgPrint(EFI_D_INFO, "LS7A pll configure done.\r\n");

//init 7a hardware
//configure to obey strict HT order
  Readl(LS7A_CONFBUS_BASE_ADDR + 0x414) |= (0x7ffff << 0);
//rewrite pci header default value
  for (i = 0; i <= 0x480; i += 0x40) {
    Readb(LS7A_CONFBUS_BASE_ADDR + 0x300c + i) = 0x0;
    Readl(LS7A_CONFBUS_BASE_ADDR + 0x303c + i) = 0x100;
  }
//rewrite HEADER TYPE to Multi-function
  Readb(LS7A_CONFBUS_BASE_ADDR + 0x304e) = 0x80;
  Readb(LS7A_CONFBUS_BASE_ADDR + 0x308e) = 0x80;
  Readb(LS7A_CONFBUS_BASE_ADDR + 0x32ce) = 0x80;
  Readb(LS7A_CONFBUS_BASE_ADDR + 0x330e) = 0x80;
  Readb(LS7A_CONFBUS_BASE_ADDR + 0x334e) = 0x80;
//fix GPU/DC header
  Readl(LS7A_CONFBUS_BASE_ADDR + 0x31c8) = 0x04000000;
  Readl(LS7A_CONFBUS_BASE_ADDR + 0x3208) = 0x03800000;

//fix LPC header
  Readl(LS7A_CONFBUS_BASE_ADDR + 0x3448) = 0x06010000;
  Readl(LS7A_CONFBUS_BASE_ADDR + 0x3460) = 0xfc000000;
  Readl(LS7A_CONFBUS_BASE_ADDR + 0x3464) = 0xfd;
  Readl(LS7A_CONFBUS_BASE_ADDR + 0x3450) = 0x10002000;
  Readl(LS7A_CONFBUS_BASE_ADDR + 0x3458) = 0x12000000;
  Readl(LS7A_CONFBUS_BASE_ADDR + 0x3444) |= 0x3;

//disable pci scan of MISC and confbus
  Readl(LS7A_CONFBUS_BASE_ADDR + 0x3800) |= 0xf;
  Readl(LS7A_CONFBUS_BASE_ADDR + 0x3878) |= 0xf;
  Readl(LS7A_CONFBUS_BASE_ADDR + 0x3000) |= 0xffffffff;
  Readl(LS7A_CONFBUS_BASE_ADDR + 0x33c0) |= 0xffffffff;
//Disable write to RO for IRQ Line bits of device header ---- start
  for (i = 0; i <= 0x90; i += 0x8) {
    Readw(LS7A_CONFBUS_BASE_ADDR + 0x3800 + i) = 0x10;
    Readl(LS7A_CONFBUS_BASE_ADDR + 0x3804 + i) = 0x10000000; /* Bit 60, PCI header offset 3Ch. Interrupt Line */
  }
//Special take care of GMEM BAR, clear to 4KB
//if defined CFG_GMEM, this Register will be reconfigured.
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_GMEM_BAR_MASK_OFFSET) = 0xfff;
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_GMEM_BAR_MASK_OFFSET + 0x4) = 0x0;
//Special take care of GPU-BAR3(64)
  Readl(LS7A_CONFBUS_BASE_ADDR + 0x3844) |= 0xfc;
//Special take care of LPC
  Readl(LS7A_CONFBUS_BASE_ADDR + 0x3888) = 0x0;
//Disable write to part of header ---- end

//change INT and HPET fix address
  Readl(LS7A_CONFBUS_BASE_ADDR + 0x460) = (INT_BASE_ADDR | 0x4);
  Readl(LS7A_CONFBUS_BASE_ADDR + 0x464) = (HPET_BASE_ADDR | 0x4);
  DbgPrint(EFI_D_INFO, "LS7A hardware init done.\r\n");
//3. device configure
  DeviceCfg(PcieInfo);
  DbgPrint(EFI_D_INFO, "\r\nLS7A init done.\r\n");

  return EFI_SUCCESS;
}


EFI_STATUS
Ls7aPcieInterruptFixup (
  VOID
  )
{
  UINT32 i;

  PCI_CONF_WRITE(3,0,0x3c,IRQ_BASE + 12);
  PCI_CONF_WRITE(3,1,0x3c,IRQ_BASE + 14);
  PCI_CONF_WRITE(4,1,0x3c,IRQ_BASE + 48);
  PCI_CONF_WRITE(4,0,0x3c,IRQ_BASE + 49);
  PCI_CONF_WRITE(5,1,0x3c,IRQ_BASE + 50);
  PCI_CONF_WRITE(5,0,0x3c,IRQ_BASE + 51);
  PCI_CONF_WRITE(6,1,0x3c,IRQ_BASE + 28);
  PCI_CONF_WRITE(6,0,0x3c,IRQ_BASE + 29);
  PCI_CONF_WRITE(7,0,0x3c,IRQ_BASE + 58);

  for (i = 0;i < 3;i++)
    PCI_CONF_WRITE(8,i,0x3c,(IRQ_BASE + i + 16));

  /*PCIE F0/F1*/
  for (i = 0; i < 6; i++) {
    if(PCI_CONF_READ((9 + i),0,0) != 0xffffffff)
    PCI_CONF_WRITE((9 + i),0,0x3c,(IRQ_BASE + 32 + i));
  }
  /*PCIE G0/G1*/
  for (i = 0; i < 4; i++) {
    if(PCI_CONF_READ((15 + i),0,0) != 0xffffffff)
    PCI_CONF_WRITE((15 + i),0,0x3c,(IRQ_BASE + 40 + i));
  }
  /*PCIE H*/
  for (i = 0; i < 2; i++) {
    if(PCI_CONF_READ((19 + i),0,0) != 0xffffffff)
    PCI_CONF_WRITE((19 + i),0,0x3c,(IRQ_BASE + 38 + i));
  }

  //Enable write to RO for IRQ Line bits of device header
  for (i = 0; i <= 0x90; i += 0x8) {
    /* Skip the mask register of GPU PCIe memroy BAR */
    if ((0x3800 + i) == CONF_GMEM_BAR_MASK_OFFSET) {
      continue;
    }
    Readw(LS7A_CONFBUS_BASE_ADDR + 0x3800 + i) = 0x10;
    Readl(LS7A_CONFBUS_BASE_ADDR + 0x3804 + i) &= ~(0x10000000); /* Bit 60, PCI header offset 3Ch, Interrupt Line */
  }

  return EFI_SUCCESS;
}
EFI_STATUS
LoongsonHtIntTransInit (
  VOID
  )
{
#ifdef LS3A4000
#ifndef MULTI_CHIP
 UINT32 NodeNum = 1;
#elif defined (DUAL_CPU)
 UINT32 NodeNum = 2;
#elif defined (QUAD_CPU)
 UINT32 NodeNum = 4;
#endif

  INT32 i;
  UINT64 Addr;
  for (i = 0;i < NodeNum;i++) {
    Addr = (((UINT64)i << 44) | LOONGSON_HT1_INT_TRANS_ADDR);
    Write64(Addr, 0x400000001fe01140ULL);
  }
#endif
  return EFI_SUCCESS;
}

EFI_STATUS
Ls7aDbg(
  VOID
  )
{
#define DBUGADDRESS 0x900000000f000000 // Memory low 240M begain

  UINT64 StoreVal, Val = 0x5a5aa5a516161616;
  UINT32 j = 0;

  //Store the memory old val
  StoreVal = Read64(DBUGADDRESS + ((UINT64)1 << 59)); //Cache Addr 0x98xx

  //Test begain
  Read64(DBUGADDRESS) = Val;
  Read64(DBUGADDRESS + ((UINT64)1 << 59)) = Val; //Cache Addr 0x98xx

  DbgPrint(EFI_D_INFO, "Read local memory .. \n");
  if (Read64(DBUGADDRESS) != Val) {
    DbgPrint(EFI_D_INFO, "local cache compare fail!\n");
    j++;
  }

  DbgPrint(EFI_D_INFO, "Read node0 7A ..\n");
  if (Read64(DBUGADDRESS + 0x00000e0000000000) != Val) {
    DbgPrint(EFI_D_INFO, "LS7A > node0 compare fail!\n");
    j++;
  }

#ifdef LS7A_2WAY_CONNECT
  UINT64 NodeId = 1;
  UINT32 Val32, ValTmp;

  DbgPrint(EFI_D_INFO, "Read node1 7A ..\n");
  if (Read64(DBUGADDRESS + (NodeId << NODE_OFFSET) + 0x00000e0000000000) != Val) {
    DbgPrint(EFI_D_INFO, "node1 > LS7A > node0 compare fail!\n");
  }

  //Store default setting
  Val32 = ValTmp = Readl(LS7A_CONFBUS_BASE_ADDR + CONF_HT_ROUTE_OFFSET);

  ValTmp &= ~(0xf000f);
  ValTmp |= 0x10001;
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_HT_ROUTE_OFFSET) = ValTmp;

  DbgPrint(EFI_D_INFO, "Read 7A node1 node0 ..\n");
  if (Read64(DBUGADDRESS + 0x00000e0000000000) != Val) {
    DbgPrint(EFI_D_INFO, "LS7A > node1 > node0 compare fail!\n");
    j++;
  }

  //Recover default setting
  Readl(LS7A_CONFBUS_BASE_ADDR + CONF_HT_ROUTE_OFFSET) = Val32;
#endif

  //Recover the memory old val
  Read64(DBUGADDRESS + ((UINT64)1 << 59)) = StoreVal; //Cache Addr 0x98xx
#undef DBUGADDRESS
  if (j != 0)
    return EFI_NOT_READY;
  else
    return EFI_SUCCESS;
}

EFI_STATUS
Ls7aPwm (
  IN SmartFan *Parameter
  )
{
  UINT32 X, Y;

  X = Parameter->MinRpm;
  Y = Parameter->MaxRpm;

  Readl(LS7A_PWM0_CTRL) &= ~1;
  Writel(LS7A_PWM0_LOW,X);
  Writel(LS7A_PWM0_FULL,Y);
  Readl(LS7A_PWM0_CTRL) |= 1;

  Readl(LS7A_PWM1_CTRL) &= ~1;
  Writel(LS7A_PWM1_LOW,X);
  Writel(LS7A_PWM1_FULL,Y);
  Readl(LS7A_PWM1_CTRL) |= 1;

  //as now, the 7A Fan control circuit(PWM2) has problem, keep it constant to avoid wearing fan.
  //readl(LS7A_PWM2_CTRL) &= ~1;
  //outl(LS7A_PWM2_LOW,x);
  //outl(LS7A_PWM2_FULL,y);
  //readl(LS7A_PWM2_CTRL) |= 1;
  Readl(LS7A_PWM2_LOW) = 0;
  Readl(LS7A_PWM2_FULL) = 100;
  Readl(LS7A_PWM2_CTRL) = 1;

  Readl(LS7A_PWM3_CTRL) &= ~1;
  Writel(LS7A_PWM3_LOW,X);
  Writel(LS7A_PWM3_FULL,Y);
  Readl(LS7A_PWM3_CTRL) |= 1;
  return EFI_SUCCESS;
}

 EFI_STATUS
W83795Fan (
  IN SmartFan *Parameter
  )
{
  UINT8 Val8, W83795Addr = 0x58;

  I2cInit(LS7A_I2C1_BASE_ADDR);

  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x05);
  Val8 |= 0xf;
  I2cWriteByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x05, Val8);
  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x05);

  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x04);

  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x0);
  Val8 |= 0x2;
  I2cWriteByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x0, Val8);
  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x0);

  Val8 = 0x0;
  I2cWriteByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x09, Val8);
  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x09);

  Val8 = 0x30;
  I2cWriteByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x0a, Val8);
  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x0a);

  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x02);
  Val8 |= 0x01;
  I2cWriteByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x02, Val8);
  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x02);

  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x03);
  Val8 |= 0x01;
  I2cWriteByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x03, Val8);
  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x02);

  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x05);
  Val8 |= 0x02;
  I2cWriteByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x05, Val8);
  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x05);

  Val8 = 65;
  I2cWriteByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x68, Val8);
  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x68);

  Val8 = 65;
  I2cWriteByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x69, Val8);
  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x69);

  Val8 = 70;
  I2cWriteByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x6b, Val8);
  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x6b);

  Val8 = 42;
  I2cWriteByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x60, Val8);
  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x60);

  Val8 = 42;
  I2cWriteByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x61, Val8);
  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x61);

  Val8 = 47;
  I2cWriteByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x63, Val8);
  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x63);

  Val8 = 0x0;
  I2cWriteByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x0f, Val8);
  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x0f);

  Val8 = 0x80;
  I2cWriteByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x28, Val8);
  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x28);

  Val8 = 0x80;
  I2cWriteByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x29, Val8);
  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x29);

  Val8 = 80;
  I2cWriteByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x20, Val8);
  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x20);

  Val8 = 80;
  I2cWriteByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x21, Val8);
  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x21);

  Val8 = 0x5;
  I2cWriteByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x0d, Val8);
  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x0d);

  Val8 = 0x5;
  I2cWriteByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x0e, Val8);
  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x0e);

  Val8 = 10;
  I2cWriteByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x30, Val8);
  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x30);

  Val8 = 10;
  I2cWriteByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x31, Val8);
  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x31);

  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x70);
  Val8 &= 0xf;
  I2cWriteByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x70, Val8);
  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x70);

  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x71);
  Val8 &= 0xf;
  I2cWriteByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x71, Val8);
  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x71);

  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x72);
  Val8 &= 0xf;
  I2cWriteByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x72, Val8);
  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x72);

  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x74);
  Val8 &= 0xf;
  I2cWriteByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x74, Val8);
  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x74);

  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x75);
  Val8 &= 0xf;
  I2cWriteByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x75, Val8);
  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x75);

  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x0);
  Val8 &= 0x80;
  I2cWriteByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x0, Val8);
  Val8 = I2cReadByte(LS7A_I2C1_BASE_ADDR, W83795Addr, 0x0);

  return EFI_SUCCESS;
}

EFI_STATUS
SmartFanInit (
  VOID
  )
{
  EFI_STATUS Status = EFI_NOT_READY;
  SmartFan *SmartFanParameter = NULL;

  DbgPrint(EFI_D_INFO, "SmartFan Init.\n");

  EFI_STATUS (*FanHandle)(SmartFan*) = NULL;
#ifndef MULTI_CHIP
  SmartFanParameter = AllocateZeroPool (sizeof (SmartFan));
  SmartFanParameter->MinRpm = 5000;
  SmartFanParameter->MaxRpm = 10000;
  FanHandle = &Ls7aPwm;
#else
#ifdef DUAL_CPU
  FanHandle = &W83795Fan;
#endif
#endif
  if (FanHandle)
    Status = FanHandle(SmartFanParameter);

  return Status;
}

/*
 * Ls7aClearPciePortIrq:
 * Clean the Ls7a internal PCIe prot useless IRQ, to workaround the bug
 * that some PCIe device will creat pseudo interrupt in kernel.
 * This code has some risk that if the PCIe PHY connect device time are
 * not enough, this funciton will lose efficacy.
 */
EFI_STATUS
Ls7aClearPciePortIrq (
  IN Ls7aPcieInfo *PcieInfo
  )
{
  INT32 i;

  for (i = 0; i < (PcieInfo -> Num); i++) {
    // This debug print can not delete,if improve the cpu freq the code
    // maybe need fixup.
    DebugPrint(EFI_D_INFO, "clear pcie 0x%lx\n", PcieInfo -> PcieCfgBuffer[i]);
    // Clear the PCIe internal port useless irq. Otherwise it maybe cause
    // the linked device irq error in kernel.
    LS7A_PCIE_CLEAR_PORT_IRQ(PcieInfo -> PcieCfgBuffer[i]);
  }
  return EFI_SUCCESS;
}

//
// Ls7A1000 chipset initialize entry point.
//
EFI_STATUS
ChipsetInit (
  UINT64 NodeNum
  )
{
  EFI_STATUS Status;
  Ls7aPcieInfo *PcieInfo = NULL;
#if (TRUSTPKG_SUPPORT != 1)  
  UINT8                              Chipid0,Chipid1 = 0;
#endif
  UINT16  DataTemp;
  DbgPrint(EFI_D_INFO, "Ls7A1000 Chipset init begain\n");
  Ls3a7aHtConfigure(NodeNum);
#ifdef DEBUG_ENABLE_LOONGSON
  DbgPrint(EFI_D_INFO, "------------------Test Ls7a access-----------------\n");
  Status = Ls7aDbg();
  if (Status) {
    DbgPrint(EFI_D_ERROR, "Test Ls7a access fail!\n");
    ASSERT(0);
  }
#endif
  PcieInfo = AllocateZeroPool (sizeof (Ls7aPcieInfo));
  Ls7aResourceConfigure (PcieInfo);
  Ls7aPcieInterruptFixup();
#ifdef LS3A4000
  LoongsonHtIntTransInit(); //kernel must open judge (LOONGSON_CPU_FEATURE_EXT_IOI)
#endif
  //uefi need init gmac phy otherwise kernel network working error
 //casey delete   Ls7aGmacPhyInit();
  //Status = SmartFanInit();
  Ls7aClearPciePortIrq(PcieInfo); // Clean Ls7a internal PCIe useless IRQ
  for(DataTemp=0;DataTemp < 1000;DataTemp++){
    LS7AGPIO_O(0) = LS7AGPIO_HIGH;
    MicroSecondDelay(1000000/6000);
    LS7AGPIO_O(0) = LS7AGPIO_LOW;
    MicroSecondDelay(1000000/6000);
  }
  #if (TRUSTPKG_SUPPORT != 1)  
  EnterConfigMode();
  EnterConfigMode();
  linux_outb (LPC_EC_INDEX_PORT, 0x20);  
  Chipid0 = linux_inb (LPC_EC_DATA_PORT);
  linux_outb (LPC_EC_INDEX_PORT, 0x21);  
  Chipid1 = linux_inb (LPC_EC_DATA_PORT); 
  DEBUG((EFI_D_ERROR, "Lyang: In Ls7a.c, line %d. Chipid0 %x Chipid1 %x\n", __LINE__,Chipid0,Chipid1));
  linux_outb (LPC_EC_INDEX_PORT, 0x7);  
  linux_outb (LPC_EC_DATA_PORT, 0x7);  
  linux_outb (LPC_EC_INDEX_PORT, 0x62); 
  linux_outb (LPC_EC_DATA_PORT, 0x04);  
  linux_outb (LPC_EC_INDEX_PORT, 0x63); 
  linux_outb (LPC_EC_DATA_PORT, 0x00); 
  linux_outb (LPC_EC_INDEX_PORT, 0x27);  
  DataTemp = linux_inb (LPC_EC_DATA_PORT);
  //DEBUG((EFI_D_ERROR, "Lyang: In EncryDriverPei.c, line %d.Data0 0x%x \n", __LINE__,Data0));
  linux_outb (LPC_EC_INDEX_PORT, 0x27);  
  linux_outb (LPC_EC_DATA_PORT, DataTemp|0x38); 
  linux_outb (LPC_EC_INDEX_PORT, 0xC2); 
  DataTemp = linux_inb (LPC_EC_DATA_PORT);
  //DEBUG((EFI_D_ERROR, "Lyang: In EncryDriverPei.c, line %d.Data0 0x%x \n", __LINE__,Data0));
  linux_outb (LPC_EC_INDEX_PORT, 0xC2);  
  linux_outb (LPC_EC_DATA_PORT, DataTemp|0x08);  
  linux_outb (LPC_EC_INDEX_PORT, 0xCA); 
  DataTemp = linux_inb (LPC_EC_DATA_PORT);
  //DEBUG((EFI_D_ERROR, "Lyang: In EncryDriverPei.c, line %d.Data1 0x%x \n", __LINE__,Data1));
//Output settings
  linux_outb (LPC_EC_INDEX_PORT, 0xCA);  
  linux_outb (LPC_EC_DATA_PORT, DataTemp|0x08);  
  DataTemp = linux_inb(0x402);
  linux_outb (0x402, (DataTemp | 0x08));
  ExitConfigMode();
#endif
  IT8733_INIT();  
  DbgPrint(EFI_D_INFO, "Ls7A1000 Chipset initialize done.\n");
  return EFI_SUCCESS;
}
